/* USER CODE BEGIN Header */
/**
  ******************************************************************************
  * File Name          : freertos.c
  * Description        : Code for freertos applications
  ******************************************************************************
  * @attention
  *
  * Copyright (c) 2024 STMicroelectronics.
  * All rights reserved.
  *
  * This software is licensed under terms that can be found in the LICENSE file
  * in the root directory of this software component.
  * If no LICENSE file comes with this software, it is provided AS-IS.
  *
  ******************************************************************************
  */
/* USER CODE END Header */

/* Includes ------------------------------------------------------------------*/
#include "FreeRTOS.h"
#include "task.h"
#include "main.h"
#include "cmsis_os.h"

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "usart.h"
#include "spi.h"
#include "tim.h"
#include "gpio.h"
#include "adc.h"
#include "temperature.h"
#include "sensor.h"
#include "m3508.h"
#include "a28_t2g4a27d1a.h"
/* USER CODE END Includes */

/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */

/* USER CODE END PTD */

/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */

/* USER CODE END PD */

/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */

/* USER CODE END PM */

/* Private variables ---------------------------------------------------------*/
/* USER CODE BEGIN Variables */
uint16_t adc2_raw[2];
/* USER CODE END Variables */
/* Definitions for defaultTask */
osThreadId_t defaultTaskHandle;
const osThreadAttr_t defaultTask_attributes = {
  .name = "defaultTask",
  .stack_size = 512 * 4,
  .priority = (osPriority_t) osPriorityLow,
};

/* Private function prototypes -----------------------------------------------*/
/* USER CODE BEGIN FunctionPrototypes */

//osThreadId_t receiveTaskHandle;
const osThreadAttr_t receiveTask_attributes = {
  .name = "receiveTask",
  .stack_size = 1024 * 4,
  .priority = (osPriority_t) osPriorityHigh,
};
void ReceiveTask(void *argument);

osThreadId_t m3508CtrlTaskHandle;
const osThreadAttr_t m3508CtrlTask_attributes = {
  .name = "m3508CtrlTask",
  .stack_size = 256 * 4,
  .priority = (osPriority_t) osPriorityAboveNormal,
};
void M3508CtrlTask(void *argument);

osThreadId_t tempCtrlTaskHandle;
const osThreadAttr_t tempCtrlTask_attributes = {
  .name = "tempCtrlTask",
  .stack_size = 512 * 4,
  .priority = (osPriority_t) osPriorityNormal,
};
void TempCtrlTask(void *argument);

osThreadId_t sensorTaskHandle;
const osThreadAttr_t sensorTask_attributes = {
  .name = "sensorTask",
  .stack_size = 256 * 4,
  .priority = (osPriority_t) osPriorityBelowNormal,
};
void SensorTask(void *argument);

osThreadId_t monitorTaskHandle;
const osThreadAttr_t monitorTask_attributes = {
  .name = "monitorTask",
  .stack_size = 256 * 4,
  .priority = (osPriority_t) osPriorityLow,
};
void MonitorTask(void *argument);
/* USER CODE END FunctionPrototypes */

void StartDefaultTask(void *argument);

void MX_FREERTOS_Init(void); /* (MISRA C 2004 rule 8.1) */

/**
  * @brief  FreeRTOS initialization
  * @param  None
  * @retval None
  */
void MX_FREERTOS_Init(void) {
  /* USER CODE BEGIN Init */

  /* USER CODE END Init */

  /* USER CODE BEGIN RTOS_MUTEX */
  /* add mutexes, ... */
  /* USER CODE END RTOS_MUTEX */

  /* USER CODE BEGIN RTOS_SEMAPHORES */
  /* add semaphores, ... */
  /* USER CODE END RTOS_SEMAPHORES */

  /* USER CODE BEGIN RTOS_TIMERS */
  /* start timers, add new ones, ... */
  /* USER CODE END RTOS_TIMERS */

  /* USER CODE BEGIN RTOS_QUEUES */
  /* add queues, ... */
  /* USER CODE END RTOS_QUEUES */

  /* Create the thread(s) */
  /* creation of defaultTask */
  defaultTaskHandle = osThreadNew(StartDefaultTask, NULL, &defaultTask_attributes);

  /* USER CODE BEGIN RTOS_THREADS */
  /* add threads, ... */
  /* USER CODE END RTOS_THREADS */

  /* USER CODE BEGIN RTOS_EVENTS */
  /* add events, ... */
  /* USER CODE END RTOS_EVENTS */

}

/* USER CODE BEGIN Header_StartDefaultTask */
/**
  * @brief  Function implementing the defaultTask thread.
  * @param  argument: Not used
  * @retval None
  */
/* USER CODE END Header_StartDefaultTask */
void StartDefaultTask(void *argument)
{
  /* USER CODE BEGIN StartDefaultTask */
  UNUSED(argument);

  // Initialize the Command functions
  UART_Cmd_Init();
  // Start UART2 DMA Receive
  A28_T2G4A27D1a_Init();

  // Add tasks to the scheduler
  receiveTaskHandle = osThreadNew(ReceiveTask, NULL, &receiveTask_attributes);
  monitorTaskHandle = osThreadNew(MonitorTask, NULL, &monitorTask_attributes);
  m3508CtrlTaskHandle = osThreadNew(M3508CtrlTask, NULL, &m3508CtrlTask_attributes);
  tempCtrlTaskHandle = osThreadNew(TempCtrlTask, NULL, &tempCtrlTask_attributes);
  sensorTaskHandle = osThreadNew(SensorTask, NULL, &sensorTask_attributes);

  osThreadTerminate(defaultTaskHandle);
  for(;;)
  {
    osDelay(1000);
  }
  /* USER CODE END StartDefaultTask */
}

/* Private application code --------------------------------------------------*/
/* USER CODE BEGIN Application */
void ReceiveTask(void *argument)
{
  UNUSED(argument);

  uint8_t cmd_id = 0;

  /* Infinite loop */
  for(;;)
  {
    osThreadFlagsWait(0x01U, osFlagsWaitAny, osWaitForever);

    // 校验接收数据的CRC8
    if (CRC8_Check(Cmd_Rx_Buffer, 64)) // 检查CRC8校验
    {
      uint8_t cmd_error[3] = {0xFF, 0x00, 0x04}; // 错误响应
      UART_Send_Cmd(cmd_error, 3); // 发送错误响应
      osThreadFlagsClear(0x01U); // 清除线程标志位
      continue; // 跳过本次循环，等待下一次接收
    }

    cmd_id = Cmd_Rx_Buffer[0]; // 获取命令ID

    int16_t tmp_var;
    float pid[3], temp_goal[4]; // 用于存储参数的数组
    uint8_t cmd_error[3] = {0xFF, 0x00, 0x08}; // 错误响应
    switch (cmd_id) // 使用 switch-case 语句处理不同的命令
    {
    case 0xA1: //SET_MOTOR_SPEED
      tmp_var = Cmd_Rx_Buffer[1] | (Cmd_Rx_Buffer[2] << 8); // 获取目标转速
      M3508_ReducerRpmControl_SetGoal((float) tmp_var * 0.01); // 设置目标转速
      break;
    
    case 0xB3: //SET_MOTOR_PID
      for (uint8_t i = 0; i < 3; i++)
      {
        tmp_var = Cmd_Rx_Buffer[1 + i * 2] | (Cmd_Rx_Buffer[2 + i * 2] << 8);
        pid[i] = (float) tmp_var * 0.01f; // 将PID参数转换为浮点数
      }
      M3508_ReducerRpmControl_Init(pid[0], pid[1], pid[2], 0.0f, 0.0f); // 初始化PID控制器
      break;
    
    case 0xA5: //SET_TEMPERATURE
      for (uint8_t i = 0; i < 4; i++)
      {
        tmp_var = Cmd_Rx_Buffer[1 + i * 2] | (Cmd_Rx_Buffer[2 + i * 2] << 8);
        temp_goal[i] = (float) tmp_var * 0.01f; // 将温度目标转换为浮点数
      }
      Temperature_Target_Set(temp_goal); // 设置温度目标
      break;
    
    case 0xB7: //SET_HEATER_PID
      for (uint8_t i = 0; i < 3; i++)
      {
        tmp_var = Cmd_Rx_Buffer[1 + i * 2] | (Cmd_Rx_Buffer[2 + i * 2] << 8);
        pid[i] = (float) tmp_var * 0.01f; // 将PID参数转换为浮点数
      }
      Temperature_PID_Set(pid); // 设置加热器PID参数
      break;

    case 0xA9: //SET_WEIGHT_OFFSET
      tmp_var = Cmd_Rx_Buffer[1] | (Cmd_Rx_Buffer[2] << 8);
      CS1237_Set_Offset(((float)tmp_var * 0.01f)); // 设置CS1237的偏移值
      break;
    
    default: // 未知命令
      UART_Send_Cmd(cmd_error, 3); // 发送错误响应
      break;
  }

    // 清除线程标志位，准备下一次接收    
    osThreadFlagsClear(0x01U);
  }
}

void M3508CtrlTask(void *argument)
{
    UNUSED(argument);

    int16_t goal_current;
    M3508_ReducerRpmControl_Init(0.0f, 0.0f, 0.0f, 0.0f, 10000.0f); // 初始化PID控制器
    M3508_CAN_Start(); // 启动M3508电机的CAN通信
    uint32_t m3508_systick = osKernelGetTickCount(); // 上传系统滴答计数器

    for(;;)
    {   
        osDelay(5);
        //osThreadFlagsWait(0x01U, osFlagsWaitAny, osWaitForever); // 等待接收任务的信号

        goal_current = M3508_ReducerRpm_Control(m3508_feedback); // 控制电流值

        M3508_SetCurrent(&M3508_CAN_HANDLE, m3508_feedback.id, goal_current); // 设置电机电流

        if ( osKernelGetTickCount() - m3508_systick > 100) // 每秒发送10次数据
        {
            m3508_systick = osKernelGetTickCount();
            uint8_t cmd_data[11];
            cmd_data[0] = 0x44; // 命令ID
            memcpy(cmd_data + 1, &m3508_feedback.rpm, 2);
            memcpy(cmd_data + 3, &m3508_feedback.actual_current, 2);
            memcpy(cmd_data + 5, &goal_current, 2);
            memcpy(cmd_data + 7, &m3508_systick, 4);
            UART_Send_Cmd(cmd_data, sizeof(cmd_data)); // 发送电机数据
        }
    }
}

void TempCtrlTask(void *argument)
{
  UNUSED(argument);
  uint32_t sys_tick_count;
  uint8_t cmd_data[21];
  Temperature_Init();

  for(;;)
  {
    sys_tick_count = osKernelGetTickCount();

    Temperature_Control();

    Temperature_Info_WriteCmd(cmd_data, sizeof(cmd_data));
    UART_Send_Cmd(cmd_data, sizeof(cmd_data));

    osDelayUntil(sys_tick_count + 100);
  }
}

void MonitorTask(void *argument)
{
  UNUSED(argument);

  int16_t vbat;
  uint8_t cmd_data[3];
  cmd_data[0] = 0x11; // 命令ID

  /* Infinite loop */
  for(;;)
  {
    // adc2_raw[0] * 0.00885693f 为电池电压，发送前放大100倍取整
    vbat = (int16_t) ((float) adc2_raw[0] * 0.885693f);
    memcpy(cmd_data + 1, &vbat, sizeof(vbat)); // 将电池电压数据复制到命令数据中
    UART_Send_Cmd(cmd_data, sizeof(cmd_data)); // 发送电池电压数据

    HAL_GPIO_TogglePin(USR_LED_GPIO_Port, USR_LED_Pin);
    osDelay(1000);
  }
}

void SensorTask(void *argument)
{
  UNUSED(argument);
  
  float weight_value = 0.0f, angle_value = 0.0f;
  int16_t weight, angle;
  uint8_t cmd_data[9];
  uint32_t sys_tick_count;

  cmd_data[0] = 0x22; // 命令ID
  CS1237_Init(); // Initialize the CS1237 ADC

  /* Infinite loop */
  for(;;)
  {
    osThreadFlagsWait(0x01U, osFlagsWaitAny, osWaitForever);

    HAL_ADC_Start_DMA(&hadc2, (uint32_t *)adc2_raw, 2);
    HAL_ADC_PollForConversion(&hadc2, 1); // 等待ADC转换完成
    angle_value = (float) adc2_raw[1] * 3.298f / 4096.0f;
    weight_value = CS1237_Read_N();
    sys_tick_count = osKernelGetTickCount();

    weight = (int16_t)(weight_value * 100.0f); // 将重量转换为整数形式
    angle = (int16_t)(angle_value * 100.0f); // 将角度转换为整数形式
    memcpy(cmd_data + 1, &weight, sizeof(weight)); // 将重量数据复制到命令数据中
    memcpy(cmd_data + 3, &angle, sizeof(angle)); // 将角度数据复制到命令数据中
    memcpy(cmd_data + 5, &sys_tick_count, sizeof(sys_tick_count)); // 将系统滴答计数器复制到命令数据中
    UART_Send_Cmd(cmd_data, sizeof(cmd_data)); // 发送重量和角度数据

    osThreadFlagsClear(0x01U);
  }
}

void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin)
{
  if (GPIO_Pin == GPIO_PIN_11)
  {
    CS1237_ReadData_EXTI();
    osThreadFlagsSet(sensorTaskHandle, 0x01U); // Notify SensorTask to send data
  }
}

/* USER CODE END Application */

